Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Set up and test Optimization

Install Package

melodic noetic   Show EOL distros: 

sudo apt-get install ros-$ROS_DISTRO-teb-local-planner

rosdep install teb_local_planner

cd ~/catkin_ws/src
git clone https://github.com/rst-tu-dortmund/teb_local_planner_tutorials.git
# install dependencies, e.g.
sudo apt-get install ros-$ROS_DISTRO-stage-ros

sudo apt-get install ros-$ROS_DISTRO-teb-local-planner-tutorials

Configure Optimization

The package includes a simple test node (test_optim_node) that optimizes a trajectory between a fixed start and goal pose. We first start configuring the planning of a single trajectory (Timed-Elastic-Band) between start and goal, afterwards we will activate and set up the planning in distinctive topologies.

Optimization of a single Trajectory

Deactivate parallel planning using the ROS parameter server (make sure to have a roscore running):

rosparam set /test_optim_node/enable_homotopy_class_planning False

Launch test_optim_node in combination with the preconfigured rviz node for visualization:

roslaunch teb_local_planner test_optim_node.launch

A new rviz window should open similar to that shown in the following figure:

Three point obstacles are included. They are represented as an interactive_markers type and therefore the obstacle configuration can be changed by clicking and holding the blue circle around each individual obstacle:

Since the Timed-Elastic-Band utilizes a local optimization scheme, the trajectory cannot transit across obstacles. This issue is addressed in the subsequent section.

But first we customize our optimization by running rqt_reconfigure:

rosrun rqt_reconfigure rqt_reconfigure

Try to customize the optimization according to your desires. But modify the parameters only slightly, since some parameter sets could lead to undesired convergence behavior or a bad performance (especially by changing the optimization parameters).

If you experience a bad performance on your system even with the default setting, try to adjust the following parameters in order to speed-up the optimization:

• Decrease no_inner_iterations

• Decrease no_outer_iterations

• Increase dt_ref

• Decrease obstacle_poses_affected

Optimization of multiple Trajectories in distinctive Topologies

We now address the problem of local optimization schemes and enable the parallel planning in distinctive topologies. This extended planner is enabled by default and requires more computational resources.

Restart roscore or reactivate the extended planner:

rosparam set /test_optim_node/enable_homotopy_class_planning True

Launch test_optim_node in combination with the preconfigured rviz node for visualization:

roslaunch teb_local_planner test_optim_node.launch

A new rviz window should open similar to that shown in the following figure:

As in the first section, all obstacles can now be moved using the computer mouse. The currently best trajectory (in sense of cheapest optimization cost) is highlighted by showing the individual poses (as red arrows) at each trajectory configuration.

Change the obstacle configuration and observe what's happening:

Again customize the optimization by running rqt_reconfigure:

rosrun rqt_reconfigure rqt_reconfigure

There exist a separate parameter section for parallel planning in distinctive topologies. Adjust the parameters according to your desires.